# can_frames.py
"""CAN帧编码和解码相关函数"""

from shared_state import (
    G_ROLL_CNT, G_READY_ROLL_CNT, G_STEERING_CAN_ID, G_STEERING_SBW_CAN_ID
)
import threading

# 如果global_lock在shared_state中定义，则从那里导入
# 否则需要在这里定义或从DCH_VR_0630导入
from shared_state import global_lock

def crc16(data):
    crc = 0xFFFF
    for byte in data:
        crc ^= (byte << 8)
        for _ in range(8):
            if crc & 0x8000:
                crc = (crc << 1) ^ 0x1021
            else:
                crc <<= 1
            crc &= 0xFFFF
    return crc

def crc8(data):
    crc = sum(data) % 256
    crc = 255 - crc
    return crc

def encode_sbw_ready_frame():
    global G_READY_ROLL_CNT
    G_READY_ROLL_CNT = (G_READY_ROLL_CNT + 1) % 0xFFFF

    data_frame = bytearray(8)
    data_frame[0] = 0x00
    data_frame[1] = 0x01
    data_frame[7] = crc8(data_frame[:-1])

    return data_frame

def encode_sbw_ffb_frame(torque=None):
    global G_ROLL_CNT

    data_frame = bytearray(64)

    if torque is None:
        torque = 0

    torque = int((torque + 20) * 50)
    torque_bin = format(torque, '013b')

    data_frame[0] = 0x20
    data_frame[1] = 0x1C
    if torque == 0:
        data_frame[2] = 0x00
    else:
        data_frame[2] = 0x01
    data_frame[3] = int(torque_bin[-4:], 2) << 4
    data_frame[4] = int(torque_bin[1:-4], 2)

    data_frame[5] = 0x4A
    data_frame[6] = 0x40
    with global_lock:
        G_ROLL_CNT = (G_ROLL_CNT + 1) % 0xFFFF

        data_frame[60] = G_ROLL_CNT & 0xFF
        data_frame[61] = (G_ROLL_CNT >> 8) & 0xFF

    crc_data = crc16(data_frame[:-2])

    data_frame[62] = crc_data & 0xFF
    data_frame[63] = (crc_data >> 8) & 0xFF

    return data_frame

def encode_switch_vr_frame(state):
    switch_vr_frame = bytearray(8)
    if state in (0x1, 0x2, 0x3):
        switch_vr_frame[0] = 1
        switch_vr_frame[1] = state

    return switch_vr_frame

def decode_steering_wheel_angle_frame(can_id, frame_data):
    if isinstance(frame_data, str):
        frame_data = [int(b, 16) for b in frame_data.strip().split()]

    if can_id == G_STEERING_CAN_ID:
        angle_raw = (frame_data[1] << 8) | frame_data[0]
        steering_rate = int(frame_data[2]) * 4
    else:
        angle_raw = (frame_data[6] << 8) | frame_data[5]
        steering_rate = int(frame_data[7]) * 4

    if angle_raw > 0x7FFF:
        angle_raw = (~angle_raw & 0xFFFF) + 1
        steering_wheel_angle = angle_raw / 10.0
    else:
        steering_wheel_angle = -angle_raw / 10.0

    return steering_wheel_angle, steering_rate

def decode_hand_force_data(frame_data):
    if len(frame_data) < 14:
        raise ValueError("CAN帧数据长度不足，无法提取手力数据")
    
    force_raw = (frame_data[13] << 8) | frame_data[12]
    
    if force_raw > 0x7FFF:
        hand_force = force_raw - 0x10000
    else:
        hand_force = force_raw
    
    return hand_force

def decode_throttle_brake_frame(frame_data):
    if isinstance(frame_data, str):
        frame_data = [int(b, 16) for b in frame_data.strip().split()]

    throttle = int(frame_data[0])
    brake = int(frame_data[1])
    return throttle, brake



def limit_and_encode_angle(angle: float, limit: float) -> int:
    limited = min(abs(angle), limit)
    signed_limited = limited if angle >= 0 else -limited
    return int((signed_limited + 1) * 90000)

def throttle_brake_response(vehicle_type, throttle, brake, speed):
    is_u7 = vehicle_type == 'U7'
    max_game_data = 0.02 if is_u7 else 0.08

    if speed < 0.5:
        return 0

    sign = 1 if brake > 10 else -1

    speed_factor = (
        0.4 if speed >= 200
        else 0.6 if speed >= 120
        else 0.8 if speed >= 60
        else 1.0
    )

    brake_factor = 0.6 if brake < 30 else 1.0

    pitch_add = max(throttle, brake) * 0.01 * max_game_data * speed_factor * brake_factor

    return pitch_add * sign


def turning_response(vehicle_type, steering_wheel_angle, steering_wheel_angle_old, steering_wheel_rate, speed):
    steering_wheel_rate = steering_wheel_rate * 6

    if abs(steering_wheel_rate) < 50 or speed < 0.5:
        return 0.0

    if speed > 200:
        speed_factor = 0.6
    elif speed > 120:
        speed_factor = 0.4
    elif speed > 60:
        speed_factor = 0.2
    elif speed > 0.5:
        speed_factor = 0.1
    else:
        speed_factor = 1

    delta_angle = steering_wheel_angle_old - steering_wheel_angle
    direction = -1 if delta_angle > 0 else 1

    if vehicle_type == 'U7':
        max_game_data = 0.02
    else:
        max_game_data = 0.09

    steering_wheel_rate = steering_wheel_rate * 0.0009
    roll_add = direction * speed_factor * max_game_data * steering_wheel_rate
    return roll_add




def encode_roll_pitch_frame(vehicle_type, throttle, brake, game_roll, game_pitch, speed, steering_wheel_angle,
                            steering_wheel_angle_old, steering_wheel_rate):
    if speed < 0.5:
        game_roll = 0
        game_pitch = 0

    roll_add = turning_response(vehicle_type,steering_wheel_angle, steering_wheel_angle_old, steering_wheel_rate, speed)
    pitch_add = throttle_brake_response(vehicle_type,throttle, brake, speed)

    is_u7 = vehicle_type == 'U7'
    roll_limit = 0.044 if is_u7 else 0.12
    pitch_limit = 0.039 if is_u7 else 0.12

    if is_u7:
        game_roll = min(abs(game_roll), 0.014)
        game_pitch = min(abs(game_pitch), 0.014)
    else :
        game_roll = min(abs(game_roll), 0.04)
        game_pitch = min(abs(game_pitch), 0.04)

    pitch_real = game_pitch * 90
    roll_total_game = roll_add + game_roll
    pitch_total_game = game_pitch + pitch_add

    roll_raw_real = game_roll * 0.5 * 90
    roll_add_real = roll_total_game * 90

    roll_int = limit_and_encode_angle(pitch_total_game, roll_limit)
    pitch_int = limit_and_encode_angle(roll_total_game, pitch_limit)

    pitch_real = pitch_total_game * 0.5 * 90

    data_frame = bytearray(8)

    pitch_bin = format(pitch_int, '020b')
    roll_bin = format(roll_int, '020b')

    data_frame[0] = 1
    data_frame[1] = int(roll_bin[12:20], 2)
    data_frame[2] = int(roll_bin[4:12], 2)
    data_frame[3] = int(roll_bin[:4], 2)

    data_frame[3] |= int(pitch_bin[16:20], 2) << 4
    data_frame[4] = int(pitch_bin[8:16], 2)
    data_frame[5] = int(pitch_bin[:8], 2)

    return data_frame, roll_raw_real, roll_add_real, pitch_real